#include "odebodywrap.h"
#include "transform.h"
#include <ode/objects.h>

namespace CoreBase
{
	ODEBodyWrap::ODEBodyWrap() : _bodyID(0), _dynamicsEnabled(false)
	{
		dMassSetSphere(&_mass, 1.f, 1.f);
		setMass(&_mass);
	}

	ODEBodyWrap::~ODEBodyWrap()
	{
		if (getBodyID() != 0)
		{
			dBodyDestroy(getBodyID());
		}
	}

	void ODEBodyWrap::setBodyID(dBodyID bodyID)
	{
		if (getBodyID() != 0)
		{
			dBodyDestroy(getBodyID());
		}
		_bodyID = bodyID;
		enableDynamics(_dynamicsEnabled);
		setMass(&_mass);
	}

	dBodyID ODEBodyWrap::getBodyID() const
	{
		return _bodyID;
	}

	void ODEBodyWrap::enableDynamics(bool enable)
	{
		_dynamicsEnabled = enable;
		if (getBodyID() != 0)
		{
			if (enable)
			{
				dBodyEnable(getBodyID());
			}
			else
			{
				dBodyDisable(getBodyID());
			}
		}
	}

	bool ODEBodyWrap::dynamicsEnabled() const
	{
		return _dynamicsEnabled;
	}

	void ODEBodyWrap::setMass(const dMass* mass)
	{
		_mass = *mass;
		if (getBodyID() != 0)
		{
			dBodySetMass(getBodyID(), &_mass);
		}
	}

	void ODEBodyWrap::getMass(dMass* mass) const
	{
		*mass = _mass;
	}

	void ODEBodyWrap::setMass(float mass)
	{
		_mass.mass = mass;

		if (getBodyID() != 0)
		{
			dBodySetMass(getBodyID(), &_mass);
		}
	}

	float ODEBodyWrap::getMass() const
	{
		return _mass.mass;
	}

	void ODEBodyWrap::setPosition(const osg::Vec3& xyz)
	{
		if (getBodyID() != 0)
		{
			dBodySetPosition(getBodyID(), xyz.x(), xyz.y(), xyz.z());
		}
	}

	void ODEBodyWrap::setRotation(const osg::Matrix& rot)
	{
		if (getBodyID() != 0)
		{
			dMatrix3 dRot;
			dRot[0] = rot(0,0);
			dRot[1] = rot(1,0);
			dRot[2] = rot(2,0);
			dRot[4] = rot(0,1);
			dRot[5] = rot(1,1);
			dRot[6] = rot(2,1);
			dRot[8] = rot(0,2);
			dRot[9] = rot(1,2);
			dRot[10] = rot(2,2);
			dBodySetRotation(getBodyID(), dRot);
		}
	}

	void ODEBodyWrap::setCenterOfGravity(const osg::Vec3& centerOfGravity)
	{
		_mass.c[0] = centerOfGravity[0];
		_mass.c[1] = centerOfGravity[1];
		_mass.c[2] = centerOfGravity[2];
		if (getBodyID() != 0)
		{
			dBodySetMass(getBodyID(), &_mass);
		}
	}

	void ODEBodyWrap::getCenterOfGravity(osg::Vec3& dest) const
	{
		dest[0] = _mass.c[0];
		dest[1] = _mass.c[1];
		dest[2] = _mass.c[2];
	}

	void ODEBodyWrap::setInertiaTensor(const osg::Matrix& inertiaTensor)
	{
		_mass.I[0] = inertiaTensor(0,0);
		_mass.I[1] = inertiaTensor(1,0);
		_mass.I[2] = inertiaTensor(2,0);
		_mass.I[4] = inertiaTensor(0,1);
		_mass.I[5] = inertiaTensor(1,1);
		_mass.I[6] = inertiaTensor(2,1);
		_mass.I[8] = inertiaTensor(0,2);
		_mass.I[9] = inertiaTensor(1,2);
		_mass.I[10] = inertiaTensor(2,2);
		if (getBodyID() != 0)
		{
			dBodySetMass(getBodyID(), &_mass);
		}
	}

	void ODEBodyWrap::getInertiaTensor(osg::Matrix& dest) const
	{
		dest(0,0) = _mass.I[0];
		dest(1,0) = _mass.I[1];
		dest(2,0) = _mass.I[2];
		dest(0,1) = _mass.I[4];
		dest(1,1) = _mass.I[5];
		dest(2,1) = _mass.I[6];
		dest(0,2) = _mass.I[8];
		dest(1,2) = _mass.I[9];
		dest(2,2) = _mass.I[10];
	}

	void ODEBodyWrap::setLinearVelocity(const osg::Vec3& newVelocity)
	{
		if (getBodyID() != 0)
		{
			dBodySetLinearVel(getBodyID(), newVelocity.x(), newVelocity.y(), newVelocity.z());
		}
	}

	osg::Vec3 ODEBodyWrap::getLinearVelocity() const
	{
		osg::Vec3 velVec;
		if (getBodyID() != 0)
		{
			const dReal* vel = dBodyGetLinearVel(getBodyID());
			velVec.set(vel[0], vel[1], vel[2]);
		}
		return velVec;
	}

	osg::Vec3 ODEBodyWrap::getAngularVelocity() const
	{
		osg::Vec3 angVec;
		if (getBodyID() != 0)
		{
			const dReal* vel = dBodyGetAngularVel(getBodyID());
			angVec.set(vel[0], vel[1], vel[2]);
		}
		return angVec;
	}

	void ODEBodyWrap::setAngularVelocity(const osg::Vec3& newVelocity)
	{
		if (getBodyID() != 0)
		{
			dBodySetAngularVel(getBodyID(), newVelocity.x(), newVelocity.y(), newVelocity.z());
		}
	}

	void ODEBodyWrap::applyForce(const osg::Vec3& force)
	{
		if (getBodyID() != 0)
		{
			dBodyAddForce(getBodyID(), force.x(), force.y(), force.z());
		}
	}

	void ODEBodyWrap::applyForceAtPos(const osg::Vec3& force, const osg::Vec3& position)
	{
		if (getBodyID() != 0)
		{
			dBodyAddForceAtPos(getBodyID(), force.x(), force.y(), force.z(),
				position.x(), position.y(), position.z());
		}
	}

	void ODEBodyWrap::applyForceAtRelPos(const osg::Vec3& force, const osg::Vec3& relPosition)
	{
		if (getBodyID() != 0)
		{
			dBodyAddForceAtRelPos(getBodyID(), force.x(), force.y(), force.z(),
				relPosition.x(), relPosition.y(), relPosition.z());
		}
	}

	void ODEBodyWrap::applyRelForce(const osg::Vec3& relForce)
	{
		if (getBodyID() != 0)
		{
			dBodyAddRelForce(getBodyID(), relForce.x(), relForce.y(), relForce.z());
		}
	}

	void ODEBodyWrap::applyRelForceAtRelPos(const osg::Vec3& relForce,
		const osg::Vec3& relPosition)
	{
		if (getBodyID() != 0)
		{
			dBodyAddRelForceAtRelPos(getBodyID(), relForce.x(), relForce.y(), relForce.z(),
				relPosition.x(), relPosition.y(), relPosition.z());
		}
	}

	void ODEBodyWrap::applyTorque(const osg::Vec3& torque)
	{
		if (getBodyID() != 0)
		{
			dBodyAddTorque(getBodyID(), torque.x(), torque.y(), torque.z());
		}
	}

	void ODEBodyWrap::applyRelTorque(const osg::Vec3& torque)
	{
		if (getBodyID() != 0)
		{
			dBodyAddRelTorque(getBodyID(), torque.x(), torque.y(), torque.z());
		}
	}

	void ODEBodyWrap::getBodyTransform(Transform& xform) const
	{
		const dReal* position = dBodyGetPosition(getBodyID());
		const dReal* rotation = dBodyGetRotation(getBodyID());
		osg::Matrix newRotation;
		newRotation(0,0) = rotation[0];
		newRotation(1,0) = rotation[1];
		newRotation(2,0) = rotation[2];
		newRotation(0,1) = rotation[4];
		newRotation(1,1) = rotation[5];
		newRotation(2,1) = rotation[6];
		newRotation(0,2) = rotation[8];
		newRotation(1,2) = rotation[9];
		newRotation(2,2) = rotation[10];
		xform.setTranslation(position[0], position[1], position[2]);
		xform.setRotation(newRotation);
	}

	void ODEBodyWrap::updateBodyTransform(const Transform& newTransform)
	{
		if (dynamicsEnabled() == false) 
			return;
		Transform odeTransform;
		getBodyTransform(odeTransform);

		if (!newTransform.epsilonEquals(odeTransform))
		{
			osg::Matrix rotation;
			osg::Vec3 position;
			newTransform.getTranslation(position);
			newTransform.getRotation(rotation);
			// Set translation
			dBodySetPosition(getBodyID(), position[0], position[1], position[2]);
			// Set rotation
			dMatrix3 dRot;
			dRot[ 0] = rotation(0,0);
			dRot[ 1] = rotation(1,0);
			dRot[ 2] = rotation(2,0);
			dRot[ 4] = rotation(0,1);
			dRot[ 5] = rotation(1,1);
			dRot[ 6] = rotation(2,1);
			dRot[ 8] = rotation(0,2);
			dRot[ 9] = rotation(1,2);
			dRot[10] = rotation(2,2);
			dBodySetRotation(getBodyID(), dRot);
		}
	}

	bool ODEBodyWrap::getAutoDisableFlag() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetAutoDisableFlag(getBodyID()) ? true : false;
		}
		return false;
	}

	void ODEBodyWrap::setAutoDisableFlag(bool auto_disable)
	{
		if (getBodyID() != 0)
		{
			dBodySetAutoDisableFlag(getBodyID(), auto_disable);
		}
	}

	float ODEBodyWrap::getAutoDisableLinearThreshold() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetAutoDisableLinearThreshold(getBodyID());
		}
		return -1.0f;
	}

	void ODEBodyWrap::setAutoDisableLinearThreshold(float threshold)
	{
		if (getBodyID() != 0)
		{
			dBodySetAutoDisableLinearThreshold(getBodyID(), threshold);
		}
	}

	float ODEBodyWrap::getAutoDisableAngularThreshold() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetAutoDisableAngularThreshold(getBodyID());
		}
		return -1.0f;
	}

	void ODEBodyWrap::setAutoDisableAngularThreshold(float threshold)
	{
		if (getBodyID() != 0)
		{
			dBodySetAutoDisableAngularThreshold(getBodyID(), threshold);
		}
	}

	int ODEBodyWrap::getAutoDisableSteps() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetAutoDisableSteps(getBodyID());
		}
		return 0;
	}

	void ODEBodyWrap::setAutoDisableSteps(int numSteps)
	{
		if (getBodyID() != 0)
		{
			dBodySetAutoDisableSteps(getBodyID(), numSteps);
		}
	}

	float ODEBodyWrap::getAutoDisableTime() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetAutoDisableTime(getBodyID());
		}
		return 0.0f;
	}

	void ODEBodyWrap::setAutoDisableTime(float numSeconds)
	{
		if (getBodyID() != 0)
		{
			dBodySetAutoDisableTime(getBodyID(), numSeconds);
		}
	}

	float ODEBodyWrap::getLinearDamping() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetLinearDamping(getBodyID());
		}
		return -1.0f;
	}

	void ODEBodyWrap::setLinearDamping(float scale)
	{
		if (getBodyID() != 0)
		{
			dBodySetLinearDamping(getBodyID(), scale);
		}
	}

	float ODEBodyWrap::getAngularDamping() const
	{
		if (getBodyID() != 0)
		{
			return dBodyGetAngularDamping(getBodyID());
		}
		return -1.0f;
	}

	void ODEBodyWrap::setAngularDamping(float scale)
	{
		if (getBodyID() != 0)
		{
			dBodySetAngularDamping(getBodyID(), scale);
		}
	}
}